/*
    Jueying.cpp - Library for JueYing intelligent vehicle code.
    Created by PINPINWA. 2024.01.05
    Released into pubilic domain
*/

#include "Arduino.h"
#include "Jueying.h"

void Jueying::initialPin()
{
    pinMode(4, OUTPUT);
    pinMode(5, OUTPUT);
    pinMode(7, OUTPUT);
    pinMode(6, OUTPUT);
    pinMode(8, OUTPUT);
    pinMode(11, OUTPUT);
    pinMode(13, OUTPUT);

    pinMode(9, INPUT);
    pinMode(A0, INPUT);
    pinMode(A1, INPUT);
    pinMode(A2, INPUT);

    pinMode(2, INPUT);
    pinMode(3, INPUT);
    pinMode(12, INPUT);
    pinMode(A3, INPUT);
    pinMode(A6, INPUT);
    pinMode(A7, INPUT);
}

void Jueying::setRightMotor(int dir, int speed)
{
    if (dir == 0){
        digitalWrite(4,LOW);
    }
    else if (dir ==1){
        digitalWrite(4,HIGH);
    }
    analogWrite(5, speed);
}

void Jueying::setLeftMotor(int dir, int speed)
{
    if (dir == 0){
        digitalWrite(7,LOW);
    }
    else if (dir ==1){
        digitalWrite(7,HIGH);
    }
    analogWrite(6, speed);
}

void Jueying::motorRun(int index, int dir, int speed)
{
    speed = abs(speed);
    if (speed>255){
        speed = 255;
    }
    if(index == 0){
        setLeftMotor(dir,speed);
    }
    else if(index == 1){
        setRightMotor(dir,speed);
    }
    else if(index == 2){
        setLeftMotor(dir,speed);
        setRightMotor(dir,speed);
    }
}

void Jueying::jueyingRun(int index, int speed)
{
    speed = abs(speed);
    if(index == 0){             //前进
        motorRun(2,0,speed);
    }    
    else if(index == 1){        //后退
        motorRun(2,1,speed);
    }
    else if(index == 2){        //左转
        motorRun(0,0,0);
        motorRun(1,0,speed);
    }
    else if(index == 3){        //右转
        motorRun(0,0,speed);
        motorRun(1,0,0);
    }
}

void Jueying::motorStop(int index)
{
    if(index == 0){             //左轮停止
        motorRun(0,0,0);
    }
    else if(index == 1){        //右轮停止
        motorRun(1,0,0);
    }
    else if(index == 2){        //左右两轮停止
        motorRun(2,0,0);
    }
}

float Jueying::e9t8UltraDis()
{
    digitalWrite(8,LOW);
    delayMicroseconds(2);
    digitalWrite(8,HIGH);
    delayMicroseconds(10);
    digitalWrite(8,LOW);
    float distance = pulseIn(9,HIGH) / 58.00;
    delay(10);
    return distance;
}

int Jueying::getIrStatus(int thValue)
{
    int irA0 = 0;
    int irA1 = 0;
    int irA2 = 0;
    int irStatus = 9;

    irA0 = analogRead(A0);
    irA1 = analogRead(A1);
    irA2 = analogRead(A2);

    if((irA0<(thValue-10))&&(irA1<(thValue-10))&&(irA2<(thValue-10))){
        irStatus = 0;
    }
    else if((irA0<(thValue-10))&&(irA1<(thValue-10))&&(irA2>=(thValue+10))){
        irStatus = 1;
    }
    else if((irA0<(thValue-10))&&(irA1>=(thValue+10))&&(irA2<(thValue-10))){
        irStatus = 2;
    }
    else if((irA0<(thValue-10))&&(irA1>=(thValue+10))&&(irA2>=(thValue+10))){
        irStatus = 3;
    }
    else if((irA0>=(thValue+10))&&(irA1<(thValue-10))&&(irA2<(thValue-10))){
        irStatus = 4;
    }
    else if((irA0>=(thValue+10))&&(irA1<(thValue-10))&&(irA2>=(thValue+10))){
        irStatus = 5;
    }
    else if((irA0>=(thValue+10))&&(irA1>=(thValue+10))&&(irA2<(thValue-10))){
        irStatus = 6;
    }
    else if((irA0>=(thValue+10))&&(irA1>=(thValue+10))&&(irA2>=(thValue+10))){
        irStatus = 7;
    }

    return irStatus;
}

boolean Jueying::isIrStatus(int status, int thValue){
    int varStatus = 8;
    varStatus = getIrStatus(thValue);
    if (varStatus == status){
        return true;
    }
    else{
        return false;
    }
}

int Jueying::getJoystickData()
{
    int jsBtnA_D2 = 0;
    int jsBtnB_D3 = 0;
    int jsBtenUp_A3 = 0;
    int jsBtnDown_A6 = 0;
    int jsBtnLeft_A7 = 0;
    int jsBtnRight_D12 = 0;
    int jsData = 0;

    jsBtnA_D2 = digitalRead(2);
    jsBtnB_D3 = digitalRead(3);
    jsBtenUp_A3 = analogRead(A3);
    jsBtnDown_A6 = analogRead(A6);
    jsBtnLeft_A7 = analogRead(A7);
    jsBtnRight_D12 = digitalRead(12);

    if(jsBtnA_D2 == 1){
        jsData = 1;
    }
    else if(jsBtnB_D3 == 1){
        jsData = 2;
    }
    else if(jsBtenUp_A3 > 500){
        jsData = 3;
    }
    else if(jsBtnDown_A6 > 500){
        jsData = 4;
    }
    else if(jsBtnLeft_A7 > 500){
        jsData = 5;
    }
    else if(jsBtnRight_D12 == 1){
        jsData = 6;
    }
    else{
        jsData = 0;
    }
    
    return jsData;

}

boolean Jueying::isJsBtnPressed(int jsbtn){
    int varJsbtn = 0;
    varJsbtn = getJoystickData();
    if (varJsbtn == jsbtn){
        return true;
    }
    else{
        return false;
    }

}